#include "sbrush_gesture_interpolator.h"
#include "dag.h"

using namespace dag;
using namespace sbrush;
using namespace std;

GestureInterpolator::GestureInterpolator() {
}

GestureInterpolator::~GestureInterpolator() throw() {
}

auto_ptr<TrainPair> GestureInterpolator::convert(const Pantomime& pantomime) {
	vector<FramePair> ioPairs;
	size_t startIdx = 0;
	
	for (size_t idx = 0; idx < pantomime.size(); idx++) {
		const InstrumentState& state = pantomime.at(idx).instrumentState;
		if (state.boolSize() < 2) {
			throw CannotHandleInstrumentState("boolSize < 2");
		}
		if (state.bools()[1] || state.bools()[0]) {
			/* found end of gesture, interpolate values between beginning and
			 * end of gesture */
			if (idx > startIdx) {
				interpolateFrames(pantomime, 
								  startIdx,
								  idx - 1,
								  ioPairs);
			}
			startIdx = idx + 1;
		}
		else if (idx == (pantomime.size() - 1)) {
			/* end of pantomime, treat last bit as a gesture even if the end of
			 * the gesture wasn't completely recorded */
			if (idx > startIdx) {
				interpolateFrames(pantomime,
								  startIdx,
								  idx, 
								  ioPairs);
			}
		}
	}
	
	if (ioPairs.size() == 0) {
		return auto_ptr<TrainPair>(NULL);
	}
	
	auto_ptr<TrainPair> tp(new TrainPair());
	FrameSpec inputSpec;
	FrameSpec outputSpec;
	inputSpec = static_cast<FrameSpec&>(*ioPairs[0].input);
	outputSpec = static_cast<FrameSpec&>(*ioPairs[0].output);
	tp->input.reset(new FrameDepot(inputSpec));
	tp->output.reset(new FrameDepot(outputSpec));
	
	for (size_t i = 0; i < ioPairs.size(); i++) {
		tp->input->add(*ioPairs[i].input);
		tp->output->add(*ioPairs[i].output);
		delete ioPairs[i].input;
		ioPairs[i].input = NULL;
		delete ioPairs[i].output;
		ioPairs[i].output = NULL;
	}
	
	return tp;
}

void GestureInterpolator::interpolateFrames(const Pantomime& pantomime,
											size_t startIdx,
											size_t endIdx,
											vector<FramePair>& ioPairs) {
	/* TODO: the code below strictly requires that every instrument state 
	 * contain the same number of bools, ints, floats, and doubles.  For now I
	 * believe this is a safe assumption, but this must be addressed.  Templates
	 * are a bit overkill, but some sort of hard-wired token check, or prototype
	 * thing may work. We don't expect instrument states to change during 
	 * runtime unless the actual physical interface changes. The code should
	 * reflect that we assume things to be static unless something strange
	 * happens.  This could also be used to help buffering by knowing exactly
	 * what the size of the instrument state is */
	
	const SpectralModel& smsModel = pantomime.getSpectralModel();
	
	/* TODO: add in first index here */
	auto_ptr<Frame> smsFrame(new Frame());
	auto_ptr<Frame> instFrame(new Frame());
	Sms::sms2Frame(smsModel.getFrame(pantomime.at(startIdx).smsFrame),
				   *smsFrame);
	pantomime.at(startIdx).instrumentState.toFrame(*instFrame);
	FramePair firstPair;
	firstPair.input = instFrame.release();
	firstPair.output = smsFrame.release();

	
	for (size_t idx = startIdx; idx < endIdx; idx++) {
		/* linearly interpolate values to provide more matches with sms frames.
		 * interpolate so that each frame between the start and end samples
		 * has a corresponding instrument state
		 */
		
		Frame startFrame, endFrame;
		const Sample& startSample = pantomime.at(idx);
		const Sample& endSample = pantomime.at(idx + 1);
		startSample.instrumentState.toFrame(startFrame);
		endSample.instrumentState.toFrame(endFrame);
		
		const double frameDiff = endSample.smsFrame - startSample.smsFrame;
		double frameInterpolateNumerator = 1.0;

		for (unsigned frame = startSample.smsFrame + 1;
			 frame < endSample.smsFrame;
			 frame++) {
			instFrame.reset(new Frame(static_cast<FrameSpec&>(startFrame)));
			const double startFrac = (frameDiff - frameInterpolateNumerator) / 
				frameDiff;
			const double endFrac = frameInterpolateNumerator / frameDiff;
			
			/* interplate bools */
			if (startFrac < 0.5) {
				memcpy(instFrame->bools, 
					   startFrame.bools, 
					   sizeof(bool) * startFrame.bSize);
			}
			else {
				memcpy(instFrame->bools, 
					   endFrame.bools, 
					   sizeof(bool) * endFrame.bSize);
			}
			/* interplate ints */
			for (size_t i = 0; i < instFrame->iSize; i++) {
				instFrame->ints[i] = (int)(startFrame.ints[i] * startFrac + 
											endFrame.ints[i] * endFrac);
			}
			/* interpolate floats */
			for (size_t i = 0; i < instFrame->fSize; i++) {
				instFrame->floats[i] = (float)(startFrame.floats[i] * startFrac + 
											endFrame.floats[i] * endFrac);
			}
			/* interpolate doubles */
			for (size_t i = 0; i < instFrame->dSize; i++) {
				instFrame->doubles[i] = (double)(startFrame.doubles[i] * startFrac + 
											endFrame.doubles[i] * endFrac);
			}
			
			smsFrame.reset(new Frame());
			Sms::sms2Frame(smsModel.getFrame(frame), *smsFrame);
			FramePair pair;
			pair.input = instFrame.release();
			pair.output = smsFrame.release();
			ioPairs.push_back(pair);
			
			frameInterpolateNumerator++;
		}
		
		/* add ending idx information */
		FramePair lastPair;
		smsFrame.reset(new Frame());
		Sms::sms2Frame(smsModel.getFrame(endSample.smsFrame), *smsFrame);
		instFrame.reset(new Frame());
		endSample.instrumentState.toFrame(*instFrame);
		lastPair.input = instFrame.release();
		lastPair.output = smsFrame.release();
		ioPairs.push_back(lastPair);
	}
}
